#ifndef __LIDAR_POINT_CLOUD_CORRECT_H__
#define __LIDAR_POINT_CLOUD_CORRECT_H__
#include <ros/ros.h>
#include "util/logger.h"
#include "common/basic_types.h"
#include "common/json.h"
#include "data_manager/data_management.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/PointField.h"
#include "pcl_conversions/pcl_conversions.h"
#include "lidar_detect/lidar_3d.h"


class LidarPointCloudCorrect
{
private:
    /* data */
public:
    LidarPointCloudCorrect(ros::NodeHandle *nodehandle);
    ~LidarPointCloudCorrect();

public:
    bool getInitState() { return initialization; }
    bool init();
    void process();

private:
    bool initConfig();
    bool pointCloudCalibration(SensorCalibrationParameterDef &param, pcl::PointCloud<pcl::PointXYZI> &pl_orig);
    pcl::PointCloud<pcl::PointXYZI> pointCloudPreProcess(pcl::PointCloud<pcl::PointXYZI> &pl_orig);
    bool pointCloudGridFilter(pcl::PointCloud<pcl::PointXYZI> point_cloud);
    void liDARGroundCluster(float dis_limit, float limit_angle, float cell_size, vector<int> &i_ground_cell_vector, vector<int> &j_ground_cell_vector);
    void oneDirectionCellJudge(float dis_limit, float angle_limit_tan, float cell_height_threshold, int m_i, int m_j, int temp_i, int temp_j,
                                                       vector<int> &i_ground_cell_vector,
                                                       vector<int> &j_ground_cell_vector);
    void groundFiltering();
    void bfsGridSearch();
    void InitPointcloud2MsgHeader(sensor_msgs::PointCloud2 &cloud);

private:
    bool initialization = false;

public:
    SensorCalibrationParameterDef sensor_correct_param_;
    ros::NodeHandle nh_;
    DataManagement *dm_;
    float xLimit = 1;
    float yLimit = 1;
    lidar_detect::lidar_objects All16LiDAROb;
};

#endif /* __LIDAR_POINT_CLOUD_CORRECT_H__ */
